import Detector from "./detector";

export default class SphereCapsuleDetector extends Detector{
    constructor(swapped) {
        super(swapped);
    }
    detectImpl(result,geom1,geom2,_tf1,_tf2,cachedData) {
        let tf1=_tf1.elements,tf2=_tf2.elements;
        result.incremental = false;
        let hh2 = geom2._halfHeight;
        let r1 = geom1._radius, r2 = geom2._radius;
        let axis2X = tf2[4],axis2Y = tf2[7],axis2Z = tf2[10];
        let cp1X = tf1[0],cp1Y = tf1[1],cp1Z = tf1[2];
        let p2X = tf2[0] + axis2X * -hh2,p2Y = tf2[1] + axis2Y * -hh2,p2Z = tf2[2] + axis2Z * -hh2;
        let q2X = tf2[0] + axis2X * hh2,q2Y = tf2[1] + axis2Y * hh2,q2Z = tf2[2] + axis2Z * hh2;
        let p12X = cp1X - p2X,p12Y = cp1Y - p2Y,p12Z = cp1Z - p2Z;
        let d2X = q2X - p2X,d2Y = q2Y - p2Y,d2Z = q2Z - p2Z;
        let d22 = hh2 * hh2 * 4;
        let t = p12X * d2X + p12Y * d2Y + p12Z * d2Z;
        if(t < 0) {
            t = 0;
        } else if(t > d22) {
            t = 1;
        } else {
            t /= d22;
        }
        let cp2X = p2X + d2X * t,cp2Y = p2Y + d2Y * t,cp2Z = p2Z + d2Z * t;
        let dX = cp1X - cp2X,dY = cp1Y - cp2Y,dZ = cp1Z - cp2Z;
        let len2 = dX * dX + dY * dY + dZ * dZ;
        if(len2 >= (r1 + r2) * (r1 + r2)) {
            return;
        }
        let len = Math.sqrt(len2);
        let nX, nY, nZ;
        if(len > 0) {
            nX = dX * (1 / len);nY = dY * (1 / len);nZ = dZ * (1 / len);
        } else {
            nX = 1;nY = 0;nZ = 0;
        }
        this.setNormal(result,nX,nY,nZ);
        let pos1X = cp1X + nX * -r1,pos1Y = cp1Y + nY * -r1,pos1Z = cp1Z + nZ * -r1;
        let pos2X = cp2X + nX * r2,pos2Y = cp2Y + nY * r2,pos2Z = cp2Z + nZ * r2;
        this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,r1 + r2 - len,0);
    }
}